% calculate the 3D position of COM 

function ret = cal_CoM3D(xpos,ypos,zpos,humanData,stanceleg)
%% transformation 
% ypos
%%
%Calculate human parameters
mass = calcMassDist(humanData.weight);

%%
com_x = COM_Pos(xpos,mass);
com_y = COM_Pos(ypos,mass);
com_z = COM_Pos(zpos,mass);



% support point (stance ankle)
for i = 1:size(xpos.lankle_avg,1)
% stanceleg == 1 : right leg is the stance leg
if stanceleg
    % stance leg is left leg
    spt_z(i) = zpos.lankle_avg(i);
    spt_y(i) = ypos.lankle_avg(i);
    nspt_z(i) = zpos.rankle_avg(i);
    nspt_y(i) = ypos.rankle_avg(i);
    com_angle(i) = atan((com_y(i)-spt_y(i))/(com_z(i)-spt_z(i)));

    
else
    % stance leg is right leg
    spt_z(i) = zpos.rankle_avg(i);
    spt_y(i) = ypos.rankle_avg(i);
    nspt_z(i) = zpos.lankle_avg(i);
    nspt_y(i) = ypos.lankle_avg(i);
    com_angle(i) = -atan((com_y(i)-spt_y(i))/(com_z(i)-spt_z(i)));

end

% % calculate the angle
% com_angle(i) = -atan((com_z(i)-spt_z(i))/(com_y(i)-spt_y(i)));

end
% com_len(i) = pdist([com_z(i),com_y(i);spt_z(i),spt_y(i)]);

% size(com_angle)

% com_Sslope(i) = -(com_x(i)-spt_x(i))/(com_y(i)-spt_y(i));

% com_NSslope(i) = (com_x(i)-nspt_x(i))/(com_y(i)-nspt_y(i));

% end

% a = humanData.sns_brk(2);b=humanData.sns_brk(3);
ret.angle = com_angle(humanData.sns_brk(2):humanData.sns_brk(3)); % 
% ret.sslope = com_Sslope(humanData.sns_brk(2):humanData.sns_brk(3));
% ret.nsslope = com_NSslope(humanData.sns_brk(2):humanData.sns_brk(3));

end

function ret = COM_Pos(pos,mass)

% calculate the CoM from human data
alpha1 = 0.433;
alpha2 = (1-0.433);
beta1 = 1;
beta2 = 0;

for i = 1:size(pos.torso,1)
    
lfc = calcFootCenter_BothLeg(pos.lankle_avg(i),pos.ltoe(i),pos.lheel(i));
rfc = calcFootCenter_BothLeg(pos.rankle_avg(i),pos.rtoe(i),pos.rheel(i));
    
ret(i) = (mass.torso*(beta1*pos.torso(i) +beta2*pos.hip_avg(i))+...
    mass.thigh*(alpha2*pos.rhip(i)+alpha1*pos.rknee(i))...
    +mass.thigh*(alpha2*pos.lhip(i)+alpha1*pos.lknee(i))...
    +mass.calf*(alpha2*pos.rknee(i)+alpha1*pos.rankle_avg(i))...
    +mass.calf*(alpha2*pos.lknee(i)+alpha1*pos.lankle_avg(i))+...
    mass.foot*rfc+mass.foot*lfc...
    )/(mass.torso+2*mass.thigh+2*mass.calf+2*mass.foot);

end

end


function ret = calcMassDist(weight)

%Coefficients of Mass Distribution from Winter's Book
m_foot = 0.0145*weight;
m_calf = 0.0465*weight;
m_thigh = 0.1*weight;
%Torso weight is the remaining body weight
m_torso = weight - 2*(m_foot+m_calf+m_thigh);


%Convert from pounds to kg
lb_to_kg = 0.45359237;

ret.foot  = m_foot*lb_to_kg;
ret.calf  = m_calf*lb_to_kg;
ret.thigh = m_thigh*lb_to_kg; 
ret.torso = m_torso*lb_to_kg;

end


function L_fc = calcFootCenter_BothLeg(ankle,toe,heel)

L_fc = 1/3*(ankle+toe+heel) ;

end